/*
 * fixmath.c
 *
 * Copyright (c) 2010, Alan Green.
 * Contactable at <firstname>.<lastname>@gmail.com
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 * 	1.	Redistributions of source code must retain the above copyright
 * 		notice, this list of conditions and the following disclaimer.
 * 	2.	Redistributions in binary form must reproduce the above copyright
 * 		notice, this list of conditions and the following disclaimer in the
 * 		documentation and/or other materials provided with the distribution.
 *
 * THIS SOFTWARE IS PROVIDED ``AS IS'' AND ANY EXPRESS OR IMPLIED WARRANTIES,
 * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND
 * FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 * COPYRIGHT HOLDER BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
 * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT
 * OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING
 * IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY
 * OF SUCH DAMAGE.
 *
 * ===============================================
 */

#include "fixed.h"
#include <avr/pgmspace.h>

Fixed F0 = 0x0000;

//
// Multiply two numbers.
// Note: this could be a few cycles faster - the compiler's prologue and
// epilogue does some unnecessary movement of data.
Fixed fix_mul(Fixed fa, Fixed fb) {
	Fixed result;
	int8_t zero = 0x0;
	asm volatile(
			"    ; hi-a * hi-b                                                    \n"
			"    fmuls  %B[fa], %B[fb]      ; hi-a * hi-b                         \n"
			"    movw   %[result], r0       ; grab that                           \n"
			"    ; lo-a * lo-b                                                    \n"
			"    fmul   %A[fa], %A[fb]      ; lo-a * lo-b                         \n"
			"    adc    %A[result], %[zero] ; Adds carry or lo-a * lo-b to result \n"
			"    ; hi-a * lo-b                                                    \n"
			"    fmulsu %B[fa], %A[fb]      ; hi-a * lo-b (signed result)         \n"
			"    sbc    %B[result], %[zero] ; Carry goes to hi-result             \n"
			"    add    %A[result], r1      ; add hi-byte to lo-result            \n"
			"    adc    %B[result], %[zero] ; ... and ripple carry through        \n"
			"    ; lo-a * hi-b                                                    \n"
			"    fmulsu %B[fb], %A[fa]      ; lo-a * hi-b                         \n"
			"    sbc    %B[result], %[zero] ; Carry goes to hi-result             \n"
			"    add    %A[result], r1      ; add hi-byte to lo-result            \n"
			"    adc    %B[result], %[zero] ; ... and ripple carry through        \n"
			"    ; Done                                                           \n"
			"    clr   r1                   ; Clean up before return              \n"
			:
			// outputs and temps
			[result] "=w" (result),
			[zero] "+&r" (zero)
			:
			// inputs,
			[fa] "a" (fa),
			[fb] "a" (fb)
	);
	return result;
}

// Table of tan to sin conversion.
// Note that only about 3/4 of this table is used
#include "fixed_tansin.h"

Fixed fix_sin(Fixed adj, Fixed opp) {
	uint8_t negative = 0;
	if (opp < 0) {
		negative = 1;
		opp = -opp;
	}
	if (adj < 0) {
		adj = -adj;
	}
	// Shuffle bits upward until one top bit set
	if (!(opp == 0 && adj == 0)) {
		do {
			opp <<= 1;
			adj <<= 1;
		} while (!(opp & 0x8000) && !(adj &0x8000));
	}
	uint8_t tan = ((opp >> 8) & 0xf0) | ((adj >> 12) & 0xf);

	// Look up the tan in the table.
	Fixed result = (Fixed) pgm_read_word(&tan_to_sin[tan]);
	if (negative) {
		result = -result;
	}
	return result;
}
